#include "../../head/utils/DemoduleUtil.h"
#include <QDateTime>
#include <QMessageBox>
#include <QTextStream>
#include <QtDebug>
#include <QTextCodec>
#include "../../head/utils/SendSerialUtil.h"
#include "../../head/consoleform.h"
#include "../../head/mainwindow.h"

#define Q_PI         3.14159


DemoduleUtil::DemoduleUtil(QObject *parent):QObject(parent)
{

    //CountN.append(3125);
    CountN.append(6250);
    CountN.append(1953);
    CountN.append(1250);
    CountN.append(977);
    initParam();
}

void DemoduleUtil::initParam()
{
    qDebug()<<"VarRcvFilt"<<sizeof(VarRcvFilt)<<endl;



    //tempDataBuffer = (double*)malloc(RECEIVE_BUF_SIZE * sizeof(double));
//    time_t timep;
//    struct tm* nowtime;
//    char logFilePath[256] = { 0 };
//    time(&timep);						//获取从1970至今过了多少秒，存入time_t类型的timep
//    nowtime = localtime(&timep);		//用localtime将秒数转化为struct tm结构体
//    sprintf_s(logFilePath, 256, "../../../%d%02d%02d_%02d%02d.log", 1900 + nowtime->tm_year, 1 + nowtime->tm_mon, nowtime->tm_mday, nowtime->tm_hour, nowtime->tm_min);//把格式化的时间写入字符数组中

    QTextCodec *code = QTextCodec::codecForName("gb2312");
    //打开系统配置文件
    QString str_conf_url = QApplication::applicationDirPath() + "/conf/InitParam.ini";
    std::string stdConfStr = code->fromUnicode(str_conf_url).toStdString();
    char const* confFilePath = stdConfStr.c_str();

    INIFileStream = OpenINIFile(confFilePath);

    QT_TRY{
        if (INIFileStream == NULL)
        {
            printf("OPEN FAILED!!1");
            //ERROR(INIFileStream);
        }

        DemodParam = (PtrDemodParam)malloc(sizeof(VarDemodParam));
        DemodulatorParamSet(INIFileStream, C_DATA_FRAME_LEN_BIT, PressureSensorNum, &Fc_Hz_read, &Rb_bps_read, &ModuleEnableStatus, DemodParam);

        /*  //2023-2-13
        if (Fc_Hz < 1 || Fc_Hz>30)
        {
            //fprintf(logfile, "Warning!!! Fc_Hz=%g replaced by Fc_Hz_read=%g !\n", Fc_Hz, Fc_Hz_read);
            Fc_Hz = Fc_Hz_read;
        }
        if (Rb_bps < 1 || Rb_bps>10)
        {
            //fprintf(logfile, "Warning!!! Rb_bps=%g replaced by Rb_bps_read=%g !\n", Rb_bps, Rb_bps_read);
            Rb_bps = Rb_bps_read;
        }
*/


        Fc_Hz =32000000/(256*4 * CountN[Fc_Hz_read-1]) * Rb_bps_read; //3125,1953,1250,977; %  载波频率/最后一个数表示采用子信道号
        Rb_bps =32000000/(256*4* CountN[Fc_Hz_read-1]);  //3125,1953,1250,977; %  符号率

        Actual_Len = Rb_bps * SAMPNUM_PERSYMBOL;

        qDebug()<<"Fc_Hz=="<<Fc_Hz<<";;;;Rb_bps==="<<Rb_bps<<";;;;Actual_Len==="<<Actual_Len<<endl;
         //record_log("Fc_Hz%lf"+Fc_Hz);


        DecData = (double*)malloc(2 * (DemodParam->TRAINING_SEQ_LEN + MAX_RBPS) * sizeof(double));
        memset(DecData, 0, 2 * (DemodParam->TRAINING_SEQ_LEN + MAX_RBPS) * sizeof(double));

        mseqIndicator = (double*)malloc(2 * (DemodParam->TRAINING_SEQ_LEN) * sizeof(double));
        memset(mseqIndicator, 0, 2 * (DemodParam->TRAINING_SEQ_LEN) * sizeof(double));


        //为结构体指针分配内存

        DemodulatorS_PriCh = (PtrDemodulatorStruct)malloc(sizeof(VarDemodulatorStruct));

        DemodulatorS_PriCh->PreFilterStructRef = NULL;//单通道 此部分结构体指针为空指针  双通道此部分结构体指针分配内存
        DemodulatorS_PriCh->DecimatorStructRef = NULL;
        DemodulatorS_PriCh->DSNC_Struct = NULL;

        DemodulatorS_PriCh->PreFilterStruct = (PtrPreFilterStruct)malloc(sizeof(VarPreFilterStruct));
        DemodulatorS_PriCh->PreFilterStruct1 = (PtrPreFilterStruct)malloc(sizeof(VarPreFilterStruct));
        DemodulatorS_PriCh->FpumpMeasureStruct = (PoPumpStruct)malloc(sizeof(VarPumpStruct));
        DemodulatorS_PriCh->DecimatorStruct = (PtrDecimator)malloc(sizeof(VarDecimator));
        DemodulatorS_PriCh->DecimatorStruct1 = (PtrDecimator)malloc(sizeof(VarDecimator));

        DemodulatorS_PriCh->BPSK_DemodStruct = (PtrBPSKDemodStruct)malloc(sizeof(VarBPSKDemodStruct));
        DemodulatorS_PriCh->ModuleStructFSRC = (PtrFarrowStruct)malloc(sizeof(VarFarrowStruct));
        DemodulatorS_PriCh->CarrierDPLLStruct = (PtrCarrierDPLLStruct)malloc(sizeof(VarCarrierDPLLStruct));
        DemodulatorS_PriCh->ModuleStructFrameSync = (PtrFrameSyncStruct)malloc(sizeof(VarFrameSyncStruct));
        DemodulatorS_PriCh->ModuleStructFrameSyncBuf = (PtrBufStruct)malloc(sizeof(VarBufStruct));
        DemodulatorS_PriCh->ModuleStructEq = (PrtEqualizerDFEStruct)malloc(sizeof(VarEqualizerDFEStruct));
        DemodulatorS_PriCh->ChirpDetectionStruct = (PtrChirpDetectionStruct)malloc(sizeof(VarChirpDetectionStruct));//ADD chirp检测
        DemodulatorS_PriCh->MedFilterStruct = (PtrMedFilterStruct)malloc(sizeof(VarMedFilterStruct));//ADD MedFilter

        //2022.12.01


        Probe = (PtrProbe)malloc(sizeof(VarProbe));
        memset(Probe, 0, sizeof(VarProbe));

        MudSigStand = (PtrMudSig)malloc(sizeof(VarMudSig));
        memset(MudSigStand, 0, sizeof(VarMudSig));

        //为泥浆脉冲信号分配内存
        MudSigStand->Pri = NULL;
        MudSigStand->Ref = NULL;
        MudSigStand->Pri = NEW(DemodParam->ADC_Fs_Hz, double);
        MudSigStand->Ref = NEW(DemodParam->ADC_Fs_Hz, double);

        //模块初始化
        DemodulatorInit(INIFileStream,Fc_Hz_read, Fc_Hz, Rb_bps, DemodulatorS_PriCh, DemodParam, ModuleEnableStatus);

        //2023-02-03 add
        save_source_data = DemodulatorParamSetByName(INIFileStream,"SYSTEM_SETTING","SOURCE_DATA");        
        ZoomIn =  DemodulatorParamSetByName(INIFileStream,"BaseParameter","ZoomIn");
        sys_zgcd = DemodulatorParamSetByName(INIFileStream,"SYSTEM_SETTING","ZGCD");

        if (!CloseINIFile(INIFileStream))//文件使用完毕，
        {
            printf("\nclose file faild!\n");
        }

        //为Probe里的指针动态分配内存
        Probe->SigBB_Frw = NEW(2 * (DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen), double);//经过初始化后为Farrow_SigLen赋值z
        Probe->SyncedSigOut = NEW(2 * (DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen), double);
        Probe->MseqCorrOut = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, double);
        Probe->FrameSyncSig = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, int);
        Probe->CarrierLocI = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, double);
        Probe->CarrierLocQ = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, double);
        Probe->SigBB_AFC = NEW(2 * (DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen), double);
        Probe->EqSig = NEW(2 * ((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS), double);
        Probe->DecData = NEW(2 * ((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS), double);
        Probe->EqErr = NEW(2 * ((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS), double);
        Probe->SigPreFilt = NEW(DemodParam->ADC_Fs_Hz, double);
        Probe->SigMedFilt = NEW(DemodParam->ADC_Fs_Hz, double);
        Probe->SignalDenoise = NEW(DemodParam->ADC_Fs_Hz, double);



        fft_in = (double *)fftw_malloc(sizeof( double) * FRAGMENT_BUF_SIZE);
        fft_out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * FRAGMENT_BUF_SIZE);

         fifoDecByte.Buf = (double*)malloc(DEC_FIFOBYTESize * sizeof(double));
         fifoDec.Buf = (double*)malloc(DEC_FIFOSize * sizeof(double));

         memset(fifoDecByte.Buf,0,DEC_FIFOBYTESize* sizeof(double));
         fifoDecByte.nWp=0;
         fifoDecByte.nRp=0;
         fifoDecByte.nUsedCnt=0;
         fifoDecByte.deep=DEC_FIFOBYTESize;

         countDec.CoutByte = 0;
         countDec.nCoutBitCnt_ModByte = 0;
         countDec.nCoutBitCnt_ModFrame = 0;


         memset(fifoDec.Buf,0,DEC_FIFOSize* sizeof(double));
         fifoDec.nWp=0;
         fifoDec.nRp=0;
         fifoDec.nUsedCnt=0;
         fifoDec.deep=DEC_FIFOSize;

         StateN = 0;
         CourseAngle_Value=0; //%航相角
         CourseAngle_Enable=0;
         PitchAngle_Value=0; //俯仰角
         PitchAngle_Enable=0;
         RollAngle_Value=0; //横滚角
         RollAngle_Enable=0;
         Temperature_Value=0; //温度
         Temperature_Enable=0;
         ParityByte_Value=0; //校验字节
         ParityByte_Enable=0;
         ParityByte_Sum = 0;


        //打开信号文件
        //printf("reading signal file ...\n");
        //fileseek = 7993 * 1000;           //925  2019-07-09_19-32-40 BPSK - 8.00bps@24.00Hz

         for(int i = 0; i < FRAGMENT_BUF_SIZE; i++){
             DataPrimary_Temp.append(0);
         }

         for(int i = 0; i < FRAGMENT_BUF_SIZE; i++){
             Data_SignalDenoise.append(0);
         }

         for(int i = 0; i < 600; i++){
             DataPrimary_Fre_Temp.append(0);
         }

         for(int i = 0; i < 6400; i++){
             MseqCorrOut_PriCh_Temp.append(0);
         }

         for(int i = 0; i < EQ_ALL_ERR_LEN; i++){
             EqSig_PriCh_Temp_I.append(0);
             EqSig_PriCh_Temp_Q.append(0);
             EqErr_PriCh_Temp.append(0);

         }






        record_log("function initParam():");
    }QT_CATCH(QString eMessage){
        record_log("ERROR:function initParam():"+eMessage);
    }
}

void DemoduleUtil::resetParam(){
    //为Probe里的指针动态分配内存
//    //Probe->SigBB_Frw = NEW(2 * (DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen), double);//经过初始化后为Farrow_SigLen赋值z
//    memset(Probe->SigBB_Frw,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(double));

//   // Probe->SyncedSigOut = NEW(2 * (DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen), double);
//    memset(Probe->SyncedSigOut,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(double));

//    //Probe->MseqCorrOut = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, double);
//    memset(Probe->MseqCorrOut,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(double));

//    //Probe->FrameSyncSig = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, int);
//    memset(Probe->FrameSyncSig,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(int));

//    //Probe->CarrierLocI = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, double);
//    memset(Probe->CarrierLocI,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(double));

//    //Probe->CarrierLocQ = NEW(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen, double);
//    memset(Probe->CarrierLocQ,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(double));

//    //Probe->SigBB_AFC = NEW(2 * (DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen), double);
//    memset(Probe->SigBB_AFC,0,2*(DemodulatorS_PriCh->ModuleStructFSRC->Farrow_SigLen)*sizeof(double));

//    //Probe->EqSig = NEW(2 * ((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS), double);
//    memset(Probe->EqSig,0,2*((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS)*sizeof(double));


//    //Probe->DecData = NEW(2 * ((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS), double);
//    memset(Probe->DecData,0,2*((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS)*sizeof(double));

//   // Probe->EqErr = NEW(2 * ((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS), double);
//    memset(Probe->EqErr,0,2*((DemodParam->TRAINING_SEQ_LEN) + MAX_RBPS)*sizeof(double));

//    //Probe->SigPreFilt = NEW(DemodParam->ADC_Fs_Hz, double);
//    memset(Probe->SigPreFilt,0,DemodParam->ADC_Fs_Hz*sizeof(double));

//   // Probe->SigMedFilt = NEW(DemodParam->ADC_Fs_Hz, double);
//    memset(Probe->SigMedFilt,0,DemodParam->ADC_Fs_Hz*sizeof(double));

//   // Probe->SignalDenoise = NEW(DemodParam->ADC_Fs_Hz, double);
//    memset(Probe->SignalDenoise,0,DemodParam->ADC_Fs_Hz*sizeof(double));


//    DataPrimary_Temp.clear();
//    for(int i = 0; i < FRAGMENT_BUF_SIZE; i++){
//        DataPrimary_Temp.append(0);
//    }

//    Data_SignalDenoise.clear();
//    for(int i = 0; i < FRAGMENT_BUF_SIZE; i++){
//        Data_SignalDenoise.append(0);
//    }
//    DataPrimary_Fre_Temp.clear();
//    for(int i = 0; i < 600; i++){
//        DataPrimary_Fre_Temp.append(0);
//    }

//    MseqCorrOut_PriCh_Temp.clear();
//    for(int i = 0; i < 6400; i++){
//        MseqCorrOut_PriCh_Temp.append(0);
//    }
//    EqSig_PriCh_Temp_I.clear();
//    EqSig_PriCh_Temp_Q.clear();
//    EqErr_PriCh_Temp.clear();
//    for(int i = 0; i < EQ_ALL_ERR_LEN; i++){
//        EqSig_PriCh_Temp_I.append(0);
//        EqSig_PriCh_Temp_Q.append(0);
//        EqErr_PriCh_Temp.append(0);

//    }
}


//记录错误日志
void DemoduleUtil::record_log(QString message){
    //打开写日志文件
    QDateTime curDateTime = QDateTime::currentDateTime();
    QString str_log_url = baseUrl+"logs/"+curDateTime.toString("yyyy-MM-dd")+".txt";
    QTextCodec *code = QTextCodec::codecForName("gb2312");
    std::string stdlogFilePath = code->fromUnicode(str_log_url).data();
    char const* logFilePath = stdlogFilePath.c_str();
    FOPEN(logfile, logFilePath, "a+");	//保存 log信息，在程序结束时closefile

    QString logTime = curDateTime.toString("yyyy-MM-dd hh:mm:ss");
    QByteArray qaLogTime = logTime.toLatin1();
    const char *cLogTime = qaLogTime.data();

    QByteArray qaMessage = message.toLatin1();
    const char *cMessage = qaMessage.data();
    fprintf(logfile,"%s %s\n",cLogTime,cMessage);
    fclose(logfile);
}

void DemoduleUtil::demodule_angle(){
    double tempRead1,tempRead2;
    int mSeqLen = DemodParam->TRAINING_SEQ_LEN;
    double xzb = 0;

    //保存解调数据
    FILE *fid;    
    QDateTime curDateTime = QDateTime::currentDateTime();
    QString save_url = baseUrl+"source/"+curDateTime.toString("yyyy-MM-dd hh")+"_decdata.txt";
    QTextCodec *code = QTextCodec::codecForName("gb2312");

    std::string stdStr = code->fromUnicode(save_url).data();
    char const* save_url_ = stdStr.c_str();
    fid = fopen(save_url_, "a+");

     for(int i = 0 ;i < Probe->DecData_Len ; i++ ){
         fifoDec.Buf[fifoDec.nWp] = Probe->DecData[i];

         fifoDec.nWp = fifoDec.nWp+1;
         if  (fifoDec.nWp > fifoDec.deep - 1){
            fifoDec.nWp=0;
         }
         fifoDec.nUsedCnt = fifoDec.nUsedCnt+1;
     }

     if(fifoDec.nUsedCnt>=2){
         for(int i = 0 ; i < EQ_ALL_ERR_LEN/2;i++){
             xzb =  xzb + pow(sqrt(pow(EqErr_PriCh_Temp[2*i],2)+pow(EqErr_PriCh_Temp[2*i+1],2)),2);
         }

         singnalEfficiency = (double)1.0/(xzb/EQ_ALL_ERR_LEN)/1.5;
         df->showSignalInfo(singnalEfficiency); //展示信噪比
      }

    while( fifoDec.nUsedCnt > 0 ) {
        if (countDec.nCoutBitCnt_ModFrame < mSeqLen) { //输出M序列
            if (countDec.nCoutBitCnt_ModFrame == mSeqLen-1) {//M序列最后一位换行
                emit sendDeCodeData("DATA");
                fprintf(fid,"DATA \n");
            }
            fifoDec.nRp = fifoDec.nRp + 1;
            if (fifoDec.nRp > fifoDec.deep - 1) {
               fifoDec.nRp = 0;
            }
            fifoDec.nUsedCnt = fifoDec.nUsedCnt - 1;
            countDec.nCoutBitCnt_ModFrame = MOD(countDec.nCoutBitCnt_ModFrame + 1, DemodParam->DATA_FRAME_LEN_BIT);
        } else {//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%输出解调数据
            countDec.CoutByte = countDec.CoutByte + fifoDec.Buf[fifoDec.nRp] * pow(2,(countDec.nCoutBitCnt_ModByte));
            fifoDec.nRp = fifoDec.nRp + 1;
            if (fifoDec.nRp > fifoDec.deep - 1) {
                fifoDec.nRp = 0;
            }
            fifoDec.nUsedCnt = fifoDec.nUsedCnt - 1;
            if (countDec.nCoutBitCnt_ModByte == 7) { //够一个字节时输出并保存到字节FIFO_DEC_Byte内
                fifoDecByte.Buf[fifoDecByte.nWp] = countDec.CoutByte;
                fifoDecByte.nWp = MOD(++fifoDecByte.nWp,fifoDecByte.deep - 1);
                fifoDecByte.nUsedCnt = fifoDecByte.nUsedCnt + 1;
                 QString qs00 = QString("%1").arg(countDec.CoutByte,2,16,QLatin1Char('0')).toUpper();
                std::string stdqs00 = qs00.toStdString();
                char const* saveqs00_ = stdqs00.c_str();
                fprintf(fid,"%s \t",saveqs00_);         
                emit sendDeCodeData(qs00);


                countDec.CoutByte = 0;
            }
           if (countDec.nCoutBitCnt_ModFrame == DemodParam->DATA_FRAME_LEN_BIT - 1) {
                fprintf(fid,"\n");

                if(sendSerialUtil != NULL){
                    if(sendSerialUtil->serial_Connect_State){
                        QByteArray qb;
                        qb.append("\r\n");
                        sendSerialUtil->sendMessage(qb);
                    }
                }

           }
            countDec.nCoutBitCnt_ModByte = MOD(++countDec.nCoutBitCnt_ModByte,8);
            countDec.nCoutBitCnt_ModFrame = MOD(++countDec.nCoutBitCnt_ModFrame, DemodParam->DATA_FRAME_LEN_BIT);
        }

    }


    //QByteArray qb;
    while(fifoDecByte.nUsedCnt >= 2){
        tempRead1 = fifoDecByte.Buf[fifoDecByte.nRp];
        //qb.append(tempRead1);
        fifoDecByte.nRp = MOD(++fifoDecByte.nRp,fifoDecByte.deep - 1);
        tempRead2 =  fifoDecByte.Buf[fifoDecByte.nRp];
       // qb.append(tempRead2);
        fifoDecByte.nRp = MOD(++fifoDecByte.nRp,fifoDecByte.deep - 1);
        fifoDecByte.nUsedCnt = fifoDecByte.nUsedCnt - 2;
        switch(StateN){
        case 0:
            if(tempRead1 == 170 && tempRead2 == 87) //  十六进制AA  or 57;
            {
                StateN = 1;
                ParityByte_Sum = ParityByte_Sum + tempRead1 + tempRead2;
            }
            break;
        case 1:
            CourseAngle_Value = tempRead1*256 + tempRead2;
            CourseAngle_Enable = 1;
            ParityByte_Sum = ParityByte_Sum + tempRead1 + tempRead2;
            StateN = 2;
            break;
        case 2:
            PitchAngle_Value = tempRead1 * 256 +  tempRead2;
            PitchAngle_Enable = 1;
            ParityByte_Sum = ParityByte_Sum + tempRead1 + tempRead2;
            StateN = 3;
            break;
        case 3:
            RollAngle_Value = tempRead1 * 256 + tempRead2;
            RollAngle_Enable = 1;
            ParityByte_Sum = ParityByte_Sum + tempRead1 + tempRead2;
            StateN = 4;
            break;
        case 4:
            Temperature_Value = tempRead1;
            Temperature_Enable = 1;
            ParityByte_Value = tempRead2;
           // temp = ParityByte_Sum /256;
            ParityByte_Sum = MOD(ParityByte_Sum + tempRead1,256); // 奇偶校验正确;
           // if(ParityByte_Sum == ParityByte_Value){
                ParityByte_Enable =1;
           // }
            ParityByte_Sum = 0;
            StateN = 0;
            break;
        }
    }

//    if(qb.size()>2 && sendSerialUtil != NULL){ //向另一串口发送解调数据
//        if(sendSerialUtil->serial_Connect_State){
//           sendSerialUtil->sendMessage(qb);
//        }
//    }
    if(ParityByte_Enable == 1){
        //计算航向角
        CourseAngle_fy = CourseAngle_Value * 0.01;
        //temp = CourseAngle_fy/360;
        CourseAngle_fy = MOD(CourseAngle_fy,360)/180 * Q_PI;
        CourseAngle_Enable = 0;

        //计算俯仰角
        if(PitchAngle_Value >= 32768){
            PitchAngle_fy = PitchAngle_Value - 65536;
        }else{
            PitchAngle_fy = PitchAngle_Value;
        }
        PitchAngle_fy = PitchAngle_fy * 0.01 / 180 * Q_PI;
        PitchAngle_Enable = 0;

        //计算工具面角
         if(RollAngle_Value >= 32768){
             RollAngle_fy = RollAngle_Value - 65536;
         }else{
             RollAngle_fy = RollAngle_Value;
         }
         RollAngle_fy = RollAngle_fy * 0.01 /180 * Q_PI;
         RollAngle_Enable = 0;
    }

    fclose(fid);
}



void DemoduleUtil::demodule_sendDecData(){        
        int mSeqLen = DemodParam->TRAINING_SEQ_LEN;
        double xzb = 0;

        FILE *fid;
        QDateTime curDateTime = QDateTime::currentDateTime();
        QString save_url = baseUrl+"source/"+curDateTime.toString("yyyy-MM-dd hh")+"_decdata.txt";
        QTextCodec *code = QTextCodec::codecForName("gb2312");
        std::string stdStr = code->fromUnicode(save_url).data();
        char const* save_url_ = stdStr.c_str();
        fid = fopen(save_url_, "a+");

        for(int i = 0 ;i < Probe->DecData_Len ; i++ ){
            fifoDec.Buf[fifoDec.nWp] = Probe->DecData[i];

            fifoDec.nWp = fifoDec.nWp+1;
            if  (fifoDec.nWp > fifoDec.deep - 1){
                fifoDec.nWp=0;
            }
            fifoDec.nUsedCnt = fifoDec.nUsedCnt+1;
        }

        if(fifoDec.nUsedCnt>=2){
            for(int i = 0 ; i < EQ_ALL_ERR_LEN/2;i++){
                xzb =  xzb + pow(sqrt(pow(EqErr_PriCh_Temp[2*i],2)+pow(EqErr_PriCh_Temp[2*i+1],2)),2);
            }

            singnalEfficiency = (double)1.0/(xzb/EQ_ALL_ERR_LEN)/1.5;
            df->showSignalInfo(singnalEfficiency); //显示信噪比
            hf->showSignalInfo(singnalEfficiency);
        }

        while( fifoDec.nUsedCnt > 0 ) {
            if (countDec.nCoutBitCnt_ModFrame < mSeqLen) { //输出M序列

                if (countDec.nCoutBitCnt_ModFrame == mSeqLen-1) {//M序列最后一位换行
                    emit sendDeCodeData("DATA");
                    fprintf(fid,"DATA \n");
                }
                fifoDec.nRp = fifoDec.nRp + 1;
                if (fifoDec.nRp > fifoDec.deep - 1) {
                   fifoDec.nRp = 0;
                }
                fifoDec.nUsedCnt = fifoDec.nUsedCnt - 1;
                countDec.nCoutBitCnt_ModFrame = MOD(countDec.nCoutBitCnt_ModFrame + 1, DemodParam->DATA_FRAME_LEN_BIT);
            } else {//%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%输出解调数据
                countDec.CoutByte = countDec.CoutByte + fifoDec.Buf[fifoDec.nRp] * pow(2,(countDec.nCoutBitCnt_ModByte));
                fifoDec.nRp = fifoDec.nRp + 1;
                if (fifoDec.nRp > fifoDec.deep - 1) {
                    fifoDec.nRp = 0;
                }
                fifoDec.nUsedCnt = fifoDec.nUsedCnt - 1;
                if (countDec.nCoutBitCnt_ModByte == 7) { //够一个字节时输出并保存到字节FIFO_DEC_Byte内
                    fifoDecByte.Buf[fifoDecByte.nWp] = countDec.CoutByte;
                    fifoDecByte.nWp = MOD(++fifoDecByte.nWp,fifoDecByte.deep - 1);
                    fifoDecByte.nUsedCnt = fifoDecByte.nUsedCnt + 1;

                    //在console显示解调完的数据
                    QString qs00 = QString("%1").arg(countDec.CoutByte,2,16,QLatin1Char('0')).toUpper();
                    std::string stdqs00 = qs00.toStdString();
                    char const* saveqs00_ = stdqs00.c_str();
                    fprintf(fid,"%s \t",saveqs00_);
                    emit sendDeCodeData(qs00);

                    countDec.CoutByte = 0;
                }
               if (countDec.nCoutBitCnt_ModFrame == DemodParam->DATA_FRAME_LEN_BIT - 1) {
                    fprintf(fid,"\n");

               }
                countDec.nCoutBitCnt_ModByte = MOD(++countDec.nCoutBitCnt_ModByte,8);
                countDec.nCoutBitCnt_ModFrame = MOD(++countDec.nCoutBitCnt_ModFrame, DemodParam->DATA_FRAME_LEN_BIT);
            }

        }
        fclose(fid);

        if(fifoDecByte.nUsedCnt >=1){
            while(fifoDecByte.nUsedCnt >= 1){
                tempRead2 = fifoDecByte.Buf[fifoDecByte.nRp];
                fifoDecByte.nRp = MOD(++fifoDecByte.nRp,fifoDecByte.deep - 1);
                fifoDecByte.nUsedCnt = fifoDecByte.nUsedCnt - 1;


                if(tempRead1 == 170 && tempRead2 == 170){ //  十六进制AA AA;
                   // qDebug()<<"AAAA OK..."<<endl;
                    this->record_log("AAAA OK...");
                    sendDecData.clear();
                    sendDecData.append(tempRead1);
                    StateN = 1;
                }

                if(StateN == 1){
                    sendDecData.append(tempRead2);
                }

                if(sendDecData.size() > 2){
                    if(sendDecData[2] == 2 && sendDecData.size() == 5 ){ //动态数据5位
                        this->record_log("SEND DATA 5...");
                        QByteArray tempDecData;
                        QString tempStr;
                        for(int dec = 0;dec < sendDecData.size();dec ++){
                            tempDecData.append(sendDecData[dec]);
                            int ll = sendDecData[dec];
                            tempStr += QString("%1").arg(ll,2,16,QLatin1Char('0')).toUpper() + " ";
                        }
                        this->record_log(tempStr);
                        if(sendSerialUtil != NULL){
                            if(sendSerialUtil->sendSerialPort->isOpen()){
                                sendSerialUtil->sendMessage(tempDecData);
                            }else{
                                this->record_log("QIODevice::write (QSerialPort): device not open....");
                            }
                        }
                        emit sendDecCode2WorkPlat(tempDecData);
                        StateN = 0;
                        sendDecData.clear();
                    }else if(sendDecData[2] == 1 && sendDecData.size()==14){ //静态数据14位
                        this->record_log("SEND DATA 14...");
                        QByteArray tempDecData;
                        QString tempStr;
                        for(int dec = 0;dec < sendDecData.size();dec ++){
                            tempDecData.append(sendDecData[dec]);
                            int ll = sendDecData[dec];
                            tempStr += QString("%1").arg(ll,2,16,QLatin1Char('0')).toUpper() + " ";
                        }
                        this->record_log(tempStr);
                        if(sendSerialUtil != NULL){
                            if(sendSerialUtil->sendSerialPort->isOpen()){
                                sendSerialUtil->sendMessage(tempDecData);
                            }else{
                                this->record_log("QIODevice::Send QSerialPort device not open....");
                            }
                        }
                        emit sendDecCode2WorkPlat(tempDecData);
                        StateN = 0;
                        sendDecData.clear();
                    }
                }else if(sendDecData.size() > 14){
                    StateN = 0;
                    sendDecData.clear();
                }

//                if(sendDecData.size() == 5 && StateN == 2){ //动态数据5位
//                    this->record_log("SEND DATA 5...");
//                    QByteArray tempDecData;
//                    QString tempStr;
//                    for(int dec = 0;dec < sendDecData.size();dec ++){
//                        tempDecData.append(sendDecData[dec]);
//                        int ll = sendDecData[dec];
//                        tempStr += QString("%1").arg(ll,2,16,QLatin1Char('0')).toUpper() + " ";
//                    }
//                    this->record_log(tempStr);
//                    sendSerialUtil->sendMessage(tempDecData);

//                    StateN = 0;
//                    sendDecData.clear();

//                }else if(sendDecData.size()==14 && StateN == 1){ //静态数据14位
//                    //qDebug()<<"SEND DATA 14..."<<endl;
//                    this->record_log("SEND DATA 14...");
//                    QByteArray tempDecData;
//                    QString tempStr;
//                    for(int dec = 0;dec < sendDecData.size();dec ++){
//                        tempDecData.append(sendDecData[dec]);
//                        int ll = sendDecData[dec];
//                        tempStr += QString("%1").arg(ll,2,16,QLatin1Char('0')).toUpper() + " ";
//                    }
//                    this->record_log(tempStr);
//                    sendSerialUtil->sendMessage(tempDecData);

//                    StateN = 0;
//                    sendDecData.clear();
//                }else if(sendDecData.size() > 14){
//                    StateN = 0;
//                    sendDecData.clear();
//                }

                tempRead1 = tempRead2;
            }
        }
}


void DemoduleUtil::demodule(){
    QT_TRY{

        //解调地面数据
        Demodulator(MudSigStand, DemodulatorS_PriCh,  ModuleEnableStatus,  Probe, DemodParam);
        //计算钻杆极坐标

        for (int i = 0; i < Actual_Len / 2; i++)
        {
            MseqCorrOut_PriCh_Temp.append(Probe->MseqCorrOut[i]);
            MseqCorrOut_PriCh_Temp.removeFirst();

        }


        for (int i = 0; i < Probe->EqSigCnt / 2; i++)
        {
            EqErr_PriCh_Temp.append(sqrt(pow(Probe->EqErr[i * 2], 2) + pow(Probe->EqErr[i * 2 + 1], 2)));
            EqErr_PriCh_Temp.removeFirst();

            EqSig_PriCh_Temp_I.append(Probe->EqSig[i * 2]);
            EqSig_PriCh_Temp_I.removeFirst();


            EqSig_PriCh_Temp_Q.append(Probe->EqSig[i * 2 + 1]);
            EqSig_PriCh_Temp_Q.removeFirst();
        }

//        EqSig_PriCh_Temp_I.append(0);
//        EqSig_PriCh_Temp_Q.append(0);
//        EqErr_PriCh_Temp.append(0);

        for (int i = 0; i < DemodParam->ADC_Fs_Hz; i++)
        {
            DataPrimary_Temp.append(MudSigStand->Pri[i]);
            DataPrimary_Temp.removeFirst();

            Data_SignalDenoise.append(Probe->SignalDenoise[i]);
            Data_SignalDenoise.removeFirst();
        }


        //fft 采样数据解调开始
        int fft_size = DataPrimary_Temp.size();
        for (int i = 0; i <fft_size; i++){
            fft_in[i] = DataPrimary_Temp[i];
        }
        my_plan = FFTW3_H::fftw_plan_dft_r2c_1d(fft_size, fft_in, fft_out, FFTW_ESTIMATE);
        fftw_execute(my_plan);


        for( int i = 0; i < fft_size / 2 + 1; i++)
        {
            fft_out[i][0] = sqrt(pow((fft_out[i][0]), 2) + pow((fft_out[i][1]), 2))/(fft_size/2);
            fft_out[i][0] = 20*log10(fft_out[i][0]);
            DataPrimary_Fre_Temp.append(fft_out[i][0]);

            if(DataPrimary_Fre_Temp.size() > fft_size / 2 + 1){
                  DataPrimary_Fre_Temp.removeFirst();
            }
        }
        //fft 采样数据解调结束
        demodule_sendDecData();


    }
    QT_CATCH(QString eMessage){
       record_log("function demodule():"+eMessage);
   }

}

void DemoduleUtil::freeParam()
{
        QT_TRY{
       //free(fft_out);
        //free(fft_sampleData);
        if(my_plan != NULL){
            fftw_destroy_plan(my_plan);
        }
        fftw_free(fft_in);
        fftw_free(fft_out);

        Demodulator_free(DemodParam->DFE_FracEqRatio);
       // free(tempDataBuffer);
        free(DecData);
       // free(DataPrimary_buffer);
       // free(SigPreFilt_PriCh_buffer);
       // free(SignalDenoise_PriCh_buffer);
       // free(SigBB_PriCh_buffer);
       // free(SigBB_Frw_PriCh_buffer);
       // free(SyncedSigOut_PriCh_buffer);
       // free(MseqCorrOut_PriCh_buffer);
       // free(FrameSync_PriCh_buffer);
       // free(CarrierLocI_PriCh_buffer);
       // free(CarrierLocQ_PriCh_buffer);
       // free(SigBB_AFC_PriCh_buffer);
       // free(EqSig_PriCh_buffer);
       // free(DecData_PriCh_buffer);
       // free(EqErr_PriCh_buffer);


        //释放结构体动态内存
        DemodulatorDelete(DemodulatorS_PriCh, Probe, MudSigStand, DemodParam, ModuleEnableStatus);
        record_log("function freeParam():");
        fclose(logfile);
    }QT_CATCH(QString eMessage){
        record_log("function freeParam():"+eMessage);
    }
}
